#include <sstream>
#include <iostream>
#include <string>
#include <cstring>
#include <math.h>
#include <stdio.h>


#include <webots/Robot.hpp>
#include <webots/DifferentialWheels.hpp>

#include <ros/ros.h>





// %EndTag(MSG_HEADER)%



using namespace std;
using namespace webots;


#define MAX_SENSOR_NUMBER 16
#define RANGE (1024 / 2)
#define BOUND(x, a, b) (((x)<(a))?(a):((x)>(b))?(b):(x))


class RWPioDistSens
{
public:
  RWPioDistSens();
  void init(webots::DifferentialWheels*,ros::NodeHandle*,std::string);
  ~RWPioDistSens();
  void iterate();
private:
  double time_step;
  double pioneer2_matrix[16][2];
  double pioneer2_max_speed;

  DistanceSensor* DSENS[16];
  double DSENS_VAL[16];

  ros::NodeHandle* n;
  webots::DifferentialWheels* DF;

};
